//
// Created by xuwj on 2021/10/29.
//
#include "tf_publisher.h"
#include "Converter.h"

namespace VNavAssistant{

    TFPublisher::TFPublisher(string frame_id_, string child_frame_id_)
    :frame_id(frame_id_),child_frame_id(child_frame_id_){

    }

    void TFPublisher::publishTF(cv::Mat &Tpc, const string &frame, const string &child_frame, bool show_quaternion) {
        if(frame.empty() || child_frame.empty()){
            cout<<"Error: No default frames' IDs, you should set the parent and child frames.\n";
            return;
        }

        tf::Transform transform;
        tf::Quaternion q;
        // body frame
        cv::Mat t = Tpc.col(3).rowRange(0,3);
        cv::Mat rotation = Tpc.colRange(0,3).rowRange(0,3);

        transform.setOrigin(tf::Vector3(t.at<float>(0),
                                        t.at<float>(1),
                                        t.at<float>(2)));

        vector<float> vec_q = Converter::toQuaternion(rotation);
        q.setX(vec_q[0]);
        q.setY(vec_q[1]);
        q.setZ(vec_q[2]);
        q.setW(vec_q[3]);
        transform.setRotation(q);

        if (show_quaternion){
            cout<<"Now the quaternion trans from "<<child_frame_id<<" to "<<frame_id<<" is(x y z w):\n"
                <<q.x()<<"\n"
                <<q.y()<<"\n"
                <<q.z()<<"\n"
                <<q.w()<<"\n";
        }
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), frame, child_frame));
    }

    void TFPublisher::publishTF(cv::Mat &Tpc, bool show_quaternion) {
        publishTF(Tpc, frame_id, child_frame_id, show_quaternion);
    }

    void TFPublisher::publishTF(tf::Transform &tf_trans_, const string &frame, const string &child_frame) {
        br.sendTransform(tf::StampedTransform(tf_trans_,ros::Time::now(),frame,child_frame));
    }



}